#include "PathPlanner.h"
#include "LaneKeeping.h"
#include <iostream>
#include <assert.h>
#include <memory>


using Eigen::VectorXd;
using Eigen::MatrixXd;

using namespace std;

PathPlanner::PathPlanner()
{
	m_states.reserve(2); // 预分配n个元素的存储空间
	m_states.push_back(unique_ptr<LineKeeping>(new LineKeeping));
}

TrajectoryPtr PathPlanner::OptimalTrajectory (const VectorXd& currStateX6, double currTime, const SensorFusion& sensorFusion)
{
	assert(m_states.size() < 3);
	VehicleState* pNewState = nullptr;
	TrajectoryPtr pOptimalTrajectory;
	VehicleState* pCurrState = m_states.back().get();
	// lane_keep state and lane_change state 
	// 返回下个状态 和 当前状态的trajectory
	tie(pNewState, pOptimalTrajectory) = pCurrState->OptimalTrajectory(currStateX6, currTime, sensorFusion);

	if (!pNewState)
	{
		assert(m_states.size() == 2);
		m_states.pop_back();
	}
	else if (pNewState != pCurrState)
	{
		assert(m_states.size() == 1);
		m_states.push_back(VehicleStatePtr(pNewState));
	}

	return pOptimalTrajectory;
}
